#include "springConstraint.h"

void SpringConstraint::SolveVelocityConstraint()
{
	
}

void SpringConstraint::SolvePositionConstraint()
{
	const Vector2f &cA = spriteA->getPosition();
	const Vector2f &cB = spriteB->getPosition();
	const Vector2f &vA = spriteA->getVelocity();
	const Vector2f &vB = spriteB->getVelocity();

	Vector2f diff = (cB - cA);
	float dist = diff.magnitude();
	diff = diff.normalize();
	float k = 100;
	float rest = 100;
	float damp = 5;

	// Linear spring force
	Vector2f fs = k * diff * (dist-rest);
	if(fs.magnitude() > 600)
		fs = fs.normalize() * 600;

	// Linear damping force
	Vector2f fd = (damp * (vB - vA)).dot(diff) * diff;

	if(spriteA->getType() == TYPE_DYNAMIC)
		spriteA->setAccel(spriteA->getAccel() + fs);
		//spriteA->setAccel(spriteA->getAccel() + fs + fd);
	//if(spriteB->getType() == TYPE_DYNAMIC)
		//spriteB->setAccel(spriteB->getAccel() - fs + fd);
}
